#include "bsp.h"

tBNO055_Data1 BNO055_Data1;

uint8_t  tBno055_Start(I2C_HandleTypeDef *hi2c)
{
    uint8_t result;
    uint8_t retry_count = 0;

    do {
        result = tBno055_Init(hi2c);

        if (result == 1) { // 通信失败，重试
            debug_print("BNO055 communication failed\r\n");
            retry_count++;
            HAL_Delay(50); // 重试间隔
        } else {
            debug_print("BNO055 initialization successful\r\n");
            break; // 成功或其他错误，退出循环
        }
    } while (retry_count < 3);

    return result;
}


uint8_t tBno055_Init(I2C_HandleTypeDef *hi2c)
{
    uint8_t chip_id = 0;
    uint8_t config_mode = 0x00; //配置模式0x00
    uint8_t normal_power = 0x00; //电源正常模式0x00
    //	uint8_t ndof_mode = 0x0C;
    uint8_t imu_mode = 0x08;

    // 1. 读取芯片ID验证通信
    if (HAL_I2C_Mem_Read(hi2c, BNO055_I2C_ADDR, 0x00,
                         I2C_MEMADD_SIZE_8BIT, &chip_id, 1, HAL_MAX_DELAY) != HAL_OK) {
        return 1; // 通信失败
    }

    // 2. 验证芯片ID
    if (chip_id != 0xA0) {
        return 2; // 芯片ID不匹配
    }

    // 3. 切换到配置模式
    if (HAL_I2C_Mem_Write(hi2c, BNO055_I2C_ADDR, 0x3D,
                          I2C_MEMADD_SIZE_8BIT, &config_mode, 1, HAL_MAX_DELAY) != HAL_OK) {
        return 1;
    }

    HAL_Delay(20);

    // 4. 设置电源模式为正常模式
    if (HAL_I2C_Mem_Write(hi2c, BNO055_I2C_ADDR, 0x3E,
                          I2C_MEMADD_SIZE_8BIT, &normal_power, 1, HAL_MAX_DELAY) != HAL_OK) {
        return 1;
    }

    HAL_Delay(10);

    // 5. 设置操作模式为imu_mode
    if (HAL_I2C_Mem_Write(hi2c, BNO055_I2C_ADDR, 0x3D,
                          I2C_MEMADD_SIZE_8BIT, &imu_mode, 1, HAL_MAX_DELAY) != HAL_OK) {
        return 1;
    }

    HAL_Delay(30);
    // 6. 初始化数据结构
    memset(&BNO055_Data, 0, sizeof(BNO055_Data_t));
    return 0; // 成功
}

uint8_t BNO055_ReadRawData(I2C_HandleTypeDef *hi2c)
{
    uint8_t send_buffer[19]; // 进一步减少缓冲区
    uint8_t accel_buffer[6];
    uint8_t gyro_buffer[6];
    HAL_StatusTypeDef status;
	uint8_t checksum = 0;
    // 读取加速度数据
    status = HAL_I2C_Mem_Read(hi2c, BNO055_I2C_ADDR, 0x08,
                              I2C_MEMADD_SIZE_8BIT, accel_buffer, 6, HAL_MAX_DELAY);

    if (status != HAL_OK) {
        // 可以在这里添加重试逻辑或错误日志
        return 1;
    }

    // 短暂延时确保数据稳定
    HAL_Delay(1);
    // 读取陀螺仪数据
    status = HAL_I2C_Mem_Read(hi2c, BNO055_I2C_ADDR, 0x14,
                              I2C_MEMADD_SIZE_8BIT, gyro_buffer, 6, HAL_MAX_DELAY);

    if (status != HAL_OK) {
        return 2;
    }

    // 帧头
    send_buffer[0] = 0xAA;
    send_buffer[1] = 0xBB;
    
    uint32_t timestamp = HAL_GetTick();
    memcpy(&send_buffer[2], &timestamp, 4);		// 时间戳（32位）
	memcpy(&send_buffer[6], accel_buffer, 6);	// 加速度数据（int16_t）
	memcpy(&send_buffer[12], gyro_buffer, 6);	// 陀螺仪数据（int16_t原始值）
	// 加速度数据（int16_t）
//    send_buffer[6] = accel_buffer[0];
//    send_buffer[7] = accel_buffer[1];
//    send_buffer[8] = accel_buffer[2];
//    send_buffer[9] = accel_buffer[3];
//    send_buffer[10] = accel_buffer[4];
//    send_buffer[11] = accel_buffer[5];
    // 陀螺仪数据（int16_t原始值）
//    send_buffer[12] = gyro_buffer[0];
//    send_buffer[13] = gyro_buffer[1];
//    send_buffer[14] = gyro_buffer[2];
//    send_buffer[15] = gyro_buffer[3];
//    send_buffer[16] = gyro_buffer[4];
//    send_buffer[17] = gyro_buffer[5];
	
	for(int i = 0; i < 18; i++) {
    checksum ^= send_buffer[i]; // 异或校验
}
	send_buffer[18] = checksum; 
	HAL_UART_Transmit(&huart1, send_buffer, 19,10);
	
//	HAL_Delay(1000);
//    debug_print("\r\n=======================test=====================\r\n");
//    int16_t acc_x = (int16_t)((accel_buffer[1] << 8) | accel_buffer[0]);
//    int16_t acc_y = (int16_t)((accel_buffer[3] << 8) | accel_buffer[2]);
//    int16_t acc_z = (int16_t)((accel_buffer[5] << 8) | accel_buffer[4]);
//    int16_t gyro_x = (int16_t)((gyro_buffer[1] << 8) | gyro_buffer[0]);
//    int16_t gyro_y = (int16_t)((gyro_buffer[3] << 8) | gyro_buffer[2]);
//    int16_t gyro_z = (int16_t)((gyro_buffer[5] << 8) | gyro_buffer[4]);
//    float acc_x_ms2 = acc_x / 100.0f;
//    float acc_y_ms2 = acc_y / 100.0f;
//    float acc_z_ms2 = acc_z / 100.0f;
//    float gyro_x_rads = gyro_x / 16.0f * (M_PI / 180.0f);
//    float gyro_y_rads = gyro_y / 16.0f * (M_PI / 180.0f);
//    float gyro_z_rads = gyro_z / 16.0f * (M_PI / 180.0f);
//    uint8_t result = 0;

//    // 检查加速度（静止状态）
//    if (fabsf(acc_z_ms2 - 9.8f) < 0.5f &&
//            fabsf(acc_x_ms2) < 0.5f &&
//            fabsf(acc_y_ms2) < 0.5f) {
//        result |= 0x01; // 加速度数据良好
//        debug_print("ACC: VVVVVVV (X:%.2f Y:%.2f Z:%.2f)\r\n",
//                    acc_x_ms2, acc_y_ms2, acc_z_ms2);
//    } else {
//        debug_print("ACC: XXXXXXX CALIB (X:%.2f Y:%.2f Z:%.2f)\r\n",
//                    acc_x_ms2, acc_y_ms2, acc_z_ms2);
//    }

//    // 检查陀螺仪（静止状态）
//    if (fabsf(gyro_x_rads) < 0.01f &&
//            fabsf(gyro_y_rads) < 0.01f &&
//            fabsf(gyro_z_rads) < 0.01f) {
//        result |= 0x02; // 陀螺仪数据良好
//        debug_print("GYRO: VVVVVVV (X:%.4f Y:%.4f Z:%.4f rad/s)\r\n",
//                    gyro_x_rads, gyro_y_rads, gyro_z_rads);
//    } else {
//        debug_print("GYRO: XXXXXXX (X:%.4f Y:%.4f Z:%.4f rad/s)\r\n",
//                    gyro_x_rads, gyro_y_rads, gyro_z_rads);
//    }

//    debug_print("\r\n=======================test=====================\r\n");
    return 0;
}

void UART_SendData(void)
{
    uint8_t send_buffer[16]; // 进一步减少缓冲区
    // 帧头
    send_buffer[0] = 0xAA;
    send_buffer[1] = 0xBB;
    // 时间戳（低16位）
    uint32_t timestamp = HAL_GetTick();
    send_buffer[2] = timestamp & 0xFF;
    send_buffer[3] = (timestamp >> 8) & 0xFF;
    // 陀螺仪数据（int16_t原始值）
    send_buffer[4] = BNO055_Data1.AccX_L;
    send_buffer[5] = BNO055_Data1.AccX_H;
    send_buffer[6] = BNO055_Data1.AccY_L;
    send_buffer[7] = BNO055_Data1.AccY_H;
    send_buffer[8] = BNO055_Data1.AccZ_L;
    send_buffer[9] = BNO055_Data1.AccZ_H;
    // 加速度数据
    send_buffer[10] = BNO055_Data1.GyroX_L;
    send_buffer[11] = BNO055_Data1.GyroX_H;
    send_buffer[12] = BNO055_Data1.GyroY_L;
    send_buffer[13] = BNO055_Data1.GyroY_H;
    send_buffer[14] = BNO055_Data1.GyroZ_L;
    send_buffer[15] = BNO055_Data1.GyroZ_H;
    // 直接使用阻塞发送，避免DMA和互斥锁
    HAL_UART_Transmit(&huart1, send_buffer, 16, 100);
}

// 验证数据是否在合理范围内，返回校验结果
uint8_t ValidateSensorData(void)
{
    debug_print("\r\n=======================test=====================\r\n");
    int16_t acc_x = (int16_t)((BNO055_Data1.AccX_H << 8) | BNO055_Data1.AccX_L);
    int16_t acc_y = (int16_t)((BNO055_Data1.AccY_H << 8) | BNO055_Data1.AccY_L);
    int16_t acc_z = (int16_t)((BNO055_Data1.AccZ_H << 8) | BNO055_Data1.AccZ_L);
    int16_t gyro_x = (int16_t)((BNO055_Data1.GyroX_H << 8) | BNO055_Data1.GyroX_L);
    int16_t gyro_y = (int16_t)((BNO055_Data1.GyroY_H << 8) | BNO055_Data1.GyroY_L);
    int16_t gyro_z = (int16_t)((BNO055_Data1.GyroZ_H << 8) | BNO055_Data1.GyroZ_L);
    float acc_x_ms2 = acc_x / 100.0f;
    float acc_y_ms2 = acc_y / 100.0f;
    float acc_z_ms2 = acc_z / 100.0f;
    float gyro_x_rads = gyro_x / 16.0f * (M_PI / 180.0f);
    float gyro_y_rads = gyro_y / 16.0f * (M_PI / 180.0f);
    float gyro_z_rads = gyro_z / 16.0f * (M_PI / 180.0f);
    uint8_t result = 0;

    // 检查加速度（静止状态）
    if (fabsf(acc_z_ms2 - 9.8f) < 0.5f &&
            fabsf(acc_x_ms2) < 0.5f &&
            fabsf(acc_y_ms2) < 0.5f) {
        result |= 0x01; // 加速度数据良好
        debug_print("ACC: VVVVVVV (X:%.2f Y:%.2f Z:%.2f)\r\n",
                    acc_x_ms2, acc_y_ms2, acc_z_ms2);
    } else {
        debug_print("ACC: XXXXXXX CALIB (X:%.2f Y:%.2f Z:%.2f)\r\n",
                    acc_x_ms2, acc_y_ms2, acc_z_ms2);
    }

    // 检查陀螺仪（静止状态）
    if (fabsf(gyro_x_rads) < 0.01f &&
            fabsf(gyro_y_rads) < 0.01f &&
            fabsf(gyro_z_rads) < 0.01f) {
        result |= 0x02; // 陀螺仪数据良好
        debug_print("GYRO: VVVVVVV (X:%.4f Y:%.4f Z:%.4f rad/s)\r\n",
                    gyro_x_rads, gyro_y_rads, gyro_z_rads);
    } else {
        debug_print("GYRO: XXXXXXX (X:%.4f Y:%.4f Z:%.4f rad/s)\r\n",
                    gyro_x_rads, gyro_y_rads, gyro_z_rads);
    }

    debug_print("\r\n=======================test=====================\r\n");
    return result;
}

void tBno055_CheckCalibrationStatus(I2C_HandleTypeDef *hi2c)
{
    uint8_t calib_status;
    HAL_I2C_Mem_Read(hi2c, BNO055_I2C_ADDR, 0x35,
                     I2C_MEMADD_SIZE_8BIT, &calib_status, 1, HAL_MAX_DELAY);
    uint8_t sys_cal = (calib_status >> 6) & 0x03;
    uint8_t gyro_cal = (calib_status >> 4) & 0x03;
    uint8_t accel_cal = (calib_status >> 2) & 0x03;
    uint8_t mag_cal = calib_status & 0x03;
    debug_print("Calib Status - Sys:%d, Gyro:%d, Accel:%d, Mag:%d\r\n",
                sys_cal, gyro_cal, accel_cal, mag_cal);
}
